//
// Created by yu on 2023/9/24.
//
#include <ros/ros.h>
#include <yaml.h>

#include "CANFrame.h"
#include "motor_control.h"
#include "iostream"
#include "mi-motor.h"
#include "lk-motor.h"
#include "imuData.h"
#include "callBacks.h"

using namespace std;

void Motor::controlBegin()
{
    CANserialPort.write("AT+AT\r\n");
    lkMotor.motorRun(0x141);
    lkMotor.motorRun(0x142);
    miMotor.motorRun(1);
    miMotor.motorRun(2);
}

void Motor::controlStop()
{
    miMotor.motorStop(1);
    miMotor.motorStop(2);
    lkMotor.motorStop(0x141);
    lkMotor.motorStop(0x142);
}

void Motor::legReset()
{

    while (Motor::righttimesCount < 50 || Motor::lefttimesCount < 50)
    {

        miMotor.runControl(1, 1.5, 32768, 0);
        miMotor.runControl(2, -1.5, 32768, 0);

        if (miMotor.Motor1.angularVelocity < 2 && miMotor.Motor1.angularVelocity > -2)
        {
            Motor::righttimesCount++;
        }
        else
        {
            if (Motor::righttimesCount < 50)
            {
                Motor::righttimesCount = 0;
            }
        }

        if (miMotor.Motor2.angularVelocity < 2 && miMotor.Motor2.angularVelocity > -2)
        {
            Motor::lefttimesCount++;
        }
        else
        {
            if (Motor::lefttimesCount < 50)
            {
                Motor::lefttimesCount = 0;
            }
        }
    }
//  清零以防溢出
    Motor::lefttimesCount = 0;
    Motor::righttimesCount = 0;
//  记录零位
    Motor::rightZero = miMotor.rightMi.angle;
    Motor::leftZero = miMotor.leftMi.angle;
}

void Motor::balancePidControl()
{
    double Kp = balancePID.kP;
    double Ki = balancePID.kI;
    double Kd = balancePID.kD;

    double angle = ImuEuler.pitch + velocityPidControl();
    double error = angle + 6;

    balancePID.integral += error;
    balancePID.derivative = error - balancePID.prev_error;

    double pidOutput = Kp * error + Ki * balancePID.integral + Kd * balancePID.derivative;

//  ROS_INFO("%lf", ImuEuler.pitch);

    lkMotor.TorqueControl(0x141, float(-pidOutput));
    lkMotor.TorqueControl(0x142, float(pidOutput));

    balancePID.prev_error = error;
}

double Motor::velocityPidControl()
{
    double Kp = speedPID.kP;
    double Ki = speedPID.kI;
    double Kd = speedPID.kD;
    double forwardSpeed = 0.0;

//  读取遥控器的前进速度
    double forward = receiver_data.channel2 * forwardSpeed;

//  计算轮速
    double velocity = double(lkMotor.leftLK.speed - lkMotor.rightLK.speed) / 2;
    double error = velocity - forward;

    speedPID.integral += error;
    speedPID.derivative = error - speedPID.prev_error;
    speedPID.prev_error = error;

    double pidOutput = Kp * error + Ki * speedPID.integral + Kd * speedPID.derivative;

    return pidOutput;
}

void Motor::legHeightControl(int id, int angle)
{
    double Kp = leftLegPID.kP;
    double Ki = leftLegPID.kI;
    double Kd = leftLegPID.kD;

    double error;
    double pidOutput;
    if (id == 1)
    {
        error = double(angle) - double(miMotor.rightMi.angle);
        rightLegPID.integral += error;
        rightLegPID.derivative = error - rightLegPID.prev_error;
        pidOutput = Kp * error + Ki * rightLegPID.integral + Kd * rightLegPID.derivative;
        rightLegPID.prev_error = error;
        miMotor.runControl(1, float(pidOutput), 32768, 0);
    }
    if (id == 2)
    {
        error = double(angle) - double(miMotor.leftMi.angle);
        leftLegPID.integral += error;
        leftLegPID.derivative = error - leftLegPID.prev_error;
        pidOutput = Kp * error + Ki * leftLegPID.integral + Kd * leftLegPID.derivative;
        leftLegPID.prev_error = error;
        miMotor.runControl(2, float(pidOutput), 32768, 0);
    }

}

void Motor::standUp()
{
    if (ImuEuler.pitch > 0)
        lkMotor.TorqueControl(0x141, -3);
    lkMotor.TorqueControl(0x142, 3);
}

void Motor::readYaml()
{
    config = YAML::LoadFile("/home/yu/Robot-system/src/main_control/config/config.yaml");
    balancePID.kP = config["balancePID"]["kP"].as<double>();
    balancePID.kI = config["balancePID"]["kI"].as<double>();
    balancePID.kD = config["balancePID"]["kD"].as<double>();

    speedPID.kP = config["speedPID"]["kP"].as<double>();
    speedPID.kI = speedPID.kP / 200;
    speedPID.kD = config["speedPID"]["kD"].as<double>();

    leftLegPID.kP = config["legPID"]["kP"].as<double>();
    leftLegPID.kI = config["legPID"]["kI"].as<double>();
    leftLegPID.kD = config["legPID"]["kD"].as<double>();
    rightLegPID.kP = config["legPID"]["kP"].as<double>();
    rightLegPID.kI = config["legPID"]["kI"].as<double>();
    rightLegPID.kD = config["legPID"]["kD"].as<double>();
}

void Motor::pidReset()
{
    leftLegPID.derivative = 0;
    leftLegPID.integral = 0;
    leftLegPID.prev_error = 0;
    rightLegPID.derivative = 0;
    rightLegPID.integral = 0;
    rightLegPID.prev_error = 0;

    balancePID.derivative = 0;
    balancePID.integral = 0;
    balancePID.prev_error = 0;

    speedPID.derivative = 0;
    speedPID.integral = 0;
    speedPID.prev_error = 0;
}

void Motor::rollBalance()
{
//    陀螺仪平衡
    double imuRate = 50.;
//    遥控器腿高控制
    double receiverRate = 2000.;
//    输出
    double recLength = receiverRate * receiver_data.channel3 + 200;
//    double imuLength = imuRate * ImuEuler.roll;
    double imuLength = 0;
    legHeightControl(1, rightZero - int(recLength) - int(imuLength));
    legHeightControl(2, leftZero + int(recLength) - int(imuLength));
}
